//
// Created by jiang on 2022/1/1.
//
#include "pid.h"

pid_t Idpi;
pid_t Iqpi;

void PID_Init(pid_t *pHandle, uint16_t kp, uint16_t ki) {
    pHandle->enI = 1;

    pHandle->Kp = kp;
    pHandle->Ki = ki;

    pHandle->iLimit = 1000;
    pHandle->outLimit = 1000;
}

inline void PID_SetLimitIntegral(pid_t *pHandle, uint32_t limit) {
    pHandle->iLimit = limit;
}

inline void PID_SetLimitOutput(pid_t *pHandle, uint32_t limit) {
    pHandle->outLimit = limit;
}

inline void PID_ClearIntegral(pid_t *pHandle) {
    pHandle->iOut = 0;
}

int PID_Process(pid_t *pHandle, int32_t err) {

    pHandle->pOut = (pHandle->Kp * err) / 1000;

    pHandle->integral += (pHandle->Ki * err);
    pHandle->iOut = pHandle->integral / 1000000;

    pHandle->iOut = (pHandle->iOut > pHandle->iLimit) ? (pHandle->iLimit) : (pHandle->iOut);
    pHandle->iOut = (pHandle->iOut < -pHandle->iLimit) ? (-pHandle->iLimit) : (pHandle->iOut);

    pHandle->out = pHandle->pOut + pHandle->iOut;

    pHandle->out = (pHandle->out > pHandle->outLimit) ? (pHandle->outLimit) : (pHandle->out);
    pHandle->out = (pHandle->out < -pHandle->outLimit) ? (-pHandle->outLimit) : (pHandle->out);

    return pHandle->out;
}

